/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Frame3.h"
//#include <iostream> 

namespace RobotPlant { namespace Math{
	Frame3::Frame3(void)
	{
		
	}

	Frame3::Frame3(const Frame3 &arg) 
		: Translation(arg.Translation),Rotation(arg.Rotation)
	{

	}

	Frame3::Frame3(const Rotation3 &r, const Vector3 &v)
	{
		Translation = v;
		Rotation = r;
	}

	Frame3::Frame3(const Vector3 & v)
	{
		Translation = v;
		Rotation = Rotation3::Identity();
	}

	Frame3::Frame3(const Rotation3 & r)
	{
		Translation = Vector3::Zero();
		Rotation = r;
	}

	Frame3::~Frame3(void)
	{
	}

	double Frame3::operator () (const int row,const int column) const
	{
		//validate input arguments
		if( (row < 0) || (row > 3) )
		{
			throw std::exception("Argument row must be between 0 and 3.");
		}
		if( (column < 0) || (column > 3) )
		{
			throw std::exception("Argument row must be between 0 and 3.");
		}


		if (row == 3)
		{
			if (column == 3)
				return 1.0;
			else
				return 0.0;
		} 
		else 
		{
			if (column == 3) 
				return Translation[row];
			else
				return Rotation(row,column);
		}
	}

	Frame3& Frame3::operator = (const Frame3 & arg)
	{ 
		Translation = arg.Translation;
		Rotation = arg.Rotation;
		return *this;
	}

	bool operator == ( const Frame3& lhs, const Frame3& rhs )
	{
		return Equal(lhs,rhs);
	}

	bool operator != ( const Frame3& lhs, const Frame3& rhs )
	{
		return !Equal(lhs,rhs);
	}

	Vector3 operator * ( const Frame3& lhs, const Vector3& rhs )
	{
		return lhs.Rotation * rhs + lhs.Translation;
	}

	Frame3 operator *( const Frame3& lhs, const Frame3& rhs )
		// Complexity : 36M+36A
	{
		return Frame3( lhs.Rotation * rhs.Rotation, lhs.Rotation * rhs.Translation + lhs.Translation );
	}


	//Wrench operator * ( const Frame3& lhs, const Wrench& rhs )
	//{
	//	// Complexity : 24M+18A
	//	Wrench tmp;
	//	tmp.Force  = lhs.Rotation * rhs.Force;
	//	tmp.Moment = lhs.Rotation * rhs.Moment + lhs.Translation ^ tmp.Force;
	//	return tmp;
	//}

	//Velocity operator * ( const Frame3& lhs, const Velocity& rhs )
	//{
	//	// Complexity : 24M+18A
	//	Velocity tmp;
	//	tmp.AngleVel = lhs.Rotation * rhs.AngleVel;
	//	tmp.LinearVel = lhs.Rotation * rhs.LinearVel + lhs.Translation ^ tmp.AngleVel;
	//	return tmp;
	//}

	Frame3 Frame3::Inverse() const
	{
		return Frame3(Rotation.Inverse(), - Rotation.Inverse(Translation));
	}

	Vector3 Frame3::Inverse(const Vector3& arg) const
	{
		return Rotation.Inverse(arg - Translation);
	}

	Wrench Frame3::Inverse(const Wrench& arg) const
	{
		Wrench tmp;
		tmp.Force =  Rotation.Inverse(arg.Force);
		tmp.Moment = Rotation.Inverse(arg.Moment - CrossProd(Translation, arg.Force));
		return tmp;
	}

	Velocity Frame3::Inverse(const Velocity& arg) const
	{
		Velocity tmp;
		tmp.AngleVel =  Rotation.Inverse(arg.AngleVel);
		tmp.LinearVel = Rotation.Inverse(arg.LinearVel - CrossProd(Translation, arg.AngleVel));
		return tmp;
	}

	//void Frame3::Integrate(const Velocity& twist,double frequency)
	//{
	//	double n = twist.Rot.Norm() / frequency;
	//	if ( n < Epsilon)
	//	{
	//		Translation += Rotation * (twist.Vel / frequency);
	//	} 
	//	else
	//	{
	//		(*this) = (*this) * 
	//			Frame3 ( Rotation3::Rot( twist.Rot, n ),
	//					twist.Vel / frequency );
	//	}
	//}

	Frame3 Frame3::Identity()
	{
		return Frame3(Rotation3::Identity(),Vector3::Zero());
	}

	Frame3 Frame3::DH_Modified(const double a,const double alpha,const double d,const double theta)
	{
		// returns Modified Denavit-Hartenberg parameters (According to Craig)
		double ct,st,ca,sa;
		ct = cos(theta);
		st = sin(theta);
		sa = sin(alpha);
		ca = cos(alpha);
		return Frame3(Rotation3(
								ct,       -st,     0,
								st*ca,  ct*ca,   -sa,
								st*sa,  ct*sa,    ca   ),
						Vector3( a,      -sa*d,  ca*d   ));
	}

	Frame3 Frame3::DH(const double a,const double alpha,const double d,const double theta)
	{
	// returns Denavit-Hartenberg parameters (Non-Modified DH)
		double ct,st,ca,sa;
		ct = cos(theta);
		st = sin(theta);
		sa = sin(alpha);
		ca = cos(alpha);
		return Frame3(Rotation3(
							ct,    -st*ca,   st*sa,
							st,     ct*ca,  -ct*sa,
							0,        sa,      ca   ),
						Vector3( a*ct,      a*st,  d ));
	}

	bool Equal(const Frame3& a,const Frame3& b,const double eps) 
	{
		return ( Equal(a.Translation, b.Translation, eps) && Equal(a.Rotation, b.Rotation, eps) );
	}

	Frame3 AddDelta(const Frame3& a,const Velocity& da,const double dt)
	{
		return Frame3( AddDelta(a.Rotation, da.AngleVel, dt),
			AddDelta(a.Translation, da.LinearVel, dt));
	}


	Velocity Diff(const Frame3& a,const Frame3& b,const double dt)
	{
		return Velocity(
			Diff(a.Translation, b.Translation, dt),
			Diff(a.Rotation, b.Rotation, dt)
			);
	}

	void Random(Frame3& F)
	{
		Random(F.Rotation);
		Random(F.Translation);
	}


	void PosRandom(Frame3& F)
	{
		Random(F.Rotation);
		Random(F.Translation);
	}
}}
